#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include<pcl/filters/passthrough.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>

#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=100118;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

int main(int argc, char* argv[])
{
    pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
    pcl::io::loadPCDFile<PointType>("/media/st/data_fast/keti/0826/envalute/testing/velodyne/000000.pcd", *pc_tmp);
    pcl::PassThrough<PointType> pass_filter;
    pass_filter.setFilterFieldName("x");
    pass_filter.setFilterLimits(0,20);
    pass_filter.setFilterLimitsNegative(false);
    pass_filter.setInputCloud(pc_tmp);
    pass_filter.filter(*pc_tmp);
    pass_filter.setFilterFieldName("y");
    pass_filter.setFilterLimits(-10,10);
    pass_filter.setFilterLimitsNegative(false);
    pass_filter.setInputCloud(pc_tmp);
    pass_filter.filter(*pc_tmp);
    string pc_path_name_= "/media/st/data_fast/keti/0826/000000.bin";
    pcd2bin(pc_tmp,pc_path_name_);

    return 0;
}
